#include "physics.hpp"
#include <math.h>
#include <vector>
#include <iostream>
#include "en.hpp"


std::vector<particle> particle_list;

float** wind_map = NULL;
float** next_map = NULL;

float** wind_actors = NULL;
float** next_actors = NULL;

int actor_size = 20;

void add_phys(int i, int j, int num)
{
    if(wind_actors!=NULL)
    {
        wind_actors[i/actor_size][j/actor_size]+=num;
    }
}


#define IX(i,j) ((i)+(w2)*(j))

#define SWAP(x0,x) {float *tmp=x0;x0=x;x=tmp;}

int w = map_x, h = map_y;

int w2 = w+2;

float* u, *v, *u_prev, *v_prev;
float* dens, *dens_prev;


void add_source (float * x, float * s, float dt )
{
    int i, size=(w+2)*(h+2);

    for ( i=0 ; i<size ; i++ ) x[i] += dt*s[i];
}

bool is_blocked(int x, int y)
{
    if(y >= map_y -1)
        return true;
    if(x >= map_x -1)
        return true;

    float rax = (float)en::w / (float)c_map_x;
    float ray = (float)en::h / (float)c_map_y;

    return en::collision_win[(int)(x*rax)][(int)(y*ray)] == NON_PATHABLE;
}

void set_bnd ( int b, float * x )
{
    return;
    int i;

    for ( i=1 ; i<=h ; i++ )
    {
        x[IX(0 ,i)] = b==1 ? -x[IX(1,i)] : x[IX(1,i)];
        x[IX(w+1,i)] = b==1 ? -x[IX(w,i)] : x[IX(w,i)];
    }
    for( i=1; i<=w; i++)
    {
        x[IX(i,0 )] = b==2 ? -x[IX(i,1)] : x[IX(i,1)];
        x[IX(i,h+1)] = b==2 ? -x[IX(i,h)] : x[IX(i,h)];
    }

    x[IX(0 ,0 )] = 0.5*(x[IX(1,0 )]+x[IX(0 ,1)]);
    x[IX(0 ,h+1)] = 0.5*(x[IX(1,h+1)]+x[IX(0 ,h )]);
    x[IX(w+1,0 )] = 0.5*(x[IX(w,0 )]+x[IX(w+1,1)]);
    x[IX(w+1,h+1)] = 0.5*(x[IX(w,h+1)]+x[IX(w+1,h )]);
}


void diffuse ( int b, float * x, float * x0, float diff, float dt )
{
    int i, j, k;
    float a=dt*diff*h*w;

    for ( k=0 ; k<7 ; k++ )
    {
        for ( i=1 ; i<=w ; i++ )
        {
            for ( j=1 ; j<=h ; j++ )
            {
                /*if(is_blocked(i, j))
                    continue;

                float sum = 0;
                int n = 0;

                if(!is_blocked(i-1, j))
                {
                    sum+=x[IX(i-1,j)];
                    n++;
                }

                if(!is_blocked(i+1, j))
                {
                    sum+=x[IX(i+1,j)];
                    n++;
                }

                if(!is_blocked(i,j-1))
                {
                    sum+=x[IX(i,j-1)];
                    n++;
                }

                if(!is_blocked(i, j+1))
                {
                    sum+=x[IX(i,j+1)];
                    n++;
                }

                sum = (x0[IX(i,j)] + a*sum)/(1+n*a);

                x[IX(i, j)] = sum;*/


                x[IX(i,j)] = (x0[IX(i,j)] + a*(x[IX(i-1,j)]+x[IX(i+1,j)]+
                                               x[IX(i,j-1)]+x[IX(i,j+1)]))/(1+4*a);
            }
        }
            set_bnd (b, x );
    }
}

void advect ( int b, float * d, float * d0, float * u, float * v, float dt )
{
    int i, j, i0, j0, i1, j1;
    float x, y, s0, t0, s1, t1, dt0x, dt0y;

    dt0x = dt*w;
    dt0y = dt*h;
    for ( i=1 ; i<=w ; i++ )
    {
        for ( j=1 ; j<=h ; j++ )
        {
            //if(is_blocked(i,j))
            //    continue;

            x = i-dt0x*u[IX(i,j)];
            y = j-dt0y*v[IX(i,j)];
            if (x<0.5) x=0.5;
            if (x>w+0.5) x=w+ 0.5;
            i0=(int)x;
            i1=i0+1;
            if (y<0.5) y=0.5;
            if (y>h+0.5) y=h+ 0.5;
            j0=(int)y;
            j1=j0+1;
            s1 = x-i0;
            s0 = 1-s1;
            t1 = y-j0;
            t0 = 1-t1;

            d[IX(i,j)] = s0*(t0*d0[IX(i0,j0)]+t1*d0[IX(i0,j1)])+
                         s1*(t0*d0[IX(i1,j0)]+t1*d0[IX(i1,j1)]);
        }
    }
    set_bnd (b, d );
}

void dens_step (float * x, float * x0, float * u, float * v, float diff,
                float dt )
{
    add_source (x, x0, dt );
    SWAP ( x0, x );
    diffuse (0, x, x0, diff, dt );
    SWAP ( x0, x );
    advect ( 0, x, x0, u, v, dt );
}

void project (float * u, float * v, float * p, float * div )
{
    int i, j, k;
    float hx, hy;

    hx = 1.0f/w;
    hy = 1.0f/h;

    for ( i=1 ; i<=w ; i++ )
    {
        for ( j=1 ; j<=h ; j++ )
        {
            div[IX(i,j)] = -0.25f*(hx*(u[IX(i+1,j)]-u[IX(i-1,j)])+
                                  hy*(v[IX(i,j+1)]-v[IX(i,j-1)]));
            p[IX(i,j)] = 0;
        }
    }
    set_bnd (0, div );
    set_bnd (0, p );

    for ( k=0 ; k<=7 ; k++ )
    {
        for ( i=1 ; i<=w ; i++ )
        {
            for ( j=1 ; j<=h ; j++ )
            {
                p[IX(i,j)] = (div[IX(i,j)]+p[IX(i-1,j)]+p[IX(i+1,j)]+
                              p[IX(i,j-1)]+p[IX(i,j+1)])/4;
            }
        }
        set_bnd (0, p );
    }

    for ( i=1 ; i<=w ; i++ )
    {
        for ( j=1 ; j<=h ; j++ )
        {
            u[IX(i,j)] -= 0.5*(p[IX(i+1,j)]-p[IX(i-1,j)])/hx;
            v[IX(i,j)] -= 0.5*(p[IX(i,j+1)]-p[IX(i,j-1)])/hy;
        }
    }
    set_bnd (1, u );
    set_bnd (2, v );
}


void vel_step (float * u, float * v, float * u0, float * v0,
               float visc, float dt )
{
    add_source (u, u0, dt );
    add_source (v, v0, dt );
    SWAP ( u0, u );
    diffuse (1, u, u0, visc, dt );
    SWAP ( v0, v );
    diffuse (2, v, v0, visc, dt );
    project (u, v, u0, v0 );
    SWAP ( u0, u );
    SWAP ( v0, v );
    advect (1, u, u0, u0, v0, dt );
    advect (2, v, v0, u0, v0, dt );
    project (u, v, u0, v0 );
}


void physics(float ftime)
{

    int size = (c_map_x*c_map_y);
    //float u[size], v[size], u_prev[size], v_p rev[size];
    //float dens[size], dens_prev[size];

    static int init;
    if(!init)
    {
        u = new float[size]();
        v = new float[size]();
        u_prev = new float[size]();
        v_prev = new float[size]();
        dens = new float[size]();
        dens_prev = new float[size]();
        init = 1;
    }

    vel_step ( u, v, u_prev, v_prev, 0.01f, ftime);
    dens_step ( dens, dens_prev, u, v, 1000.0f, ftime);
    //draw_dens ( dens );

    SWAP(dens_prev, dens);
    SWAP(u_prev, u);
    SWAP(v_prev, v);

}
